#include "functions.h"
#include "keypoints.h"

#include <pcl/common/centroid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/segment_differences.h>


vector<PointIndices> findClusters(PointCloud<pcl::PointXYZ>::Ptr cloud, double threshold, int minClusterSize, int maxClusterSize)
{
    //cout << "Find Clusters... "<<flush;

    vector<PointIndices> cluster_indices_out;
    EuclideanClusterExtraction<PointXYZ> ec;
    ec.setClusterTolerance (threshold);
    ec.setMinClusterSize (minClusterSize);
    if (maxClusterSize<minClusterSize)
        ec.setMaxClusterSize (cloud->size());
   else
        ec.setMaxClusterSize (maxClusterSize);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices_out);

    //cout <<"OK! Clusters found: " << cluster_indices_out.size()<<endl;

    return cluster_indices_out;

}


vector<PointIndices> findClusters(PointCloud<pcl::PointXYZI>::Ptr cloud, double threshold, int minClusterSize, int maxClusterSize)
{
    //cout << "Find Clusters... "<<flush;

    vector<PointIndices> cluster_indices_out;
    EuclideanClusterExtraction<PointXYZI> ec;
    ec.setClusterTolerance (threshold);
    ec.setMinClusterSize (minClusterSize);
    if (maxClusterSize<minClusterSize)
        ec.setMaxClusterSize (cloud->size());
   else
        ec.setMaxClusterSize (maxClusterSize);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices_out);

    //cout <<"OK! Clusters found: " << cluster_indices_out.size()<<endl;

    return cluster_indices_out;

}

void keepHalfClusters(PointCloud<PointXYZI>::Ptr inputCloud,PointCloud<PointXYZI>::Ptr outputCloud,double clusterThreshold)
{


    vector<PointIndices> clusters= findClusters(inputCloud,clusterThreshold,100,15000);
    PointIndices biggestNClusters;
    int nClustersToKeep= clusters.size()/2;
    cout << "Keeping "<< nClustersToKeep<< " biggest clusters ..."<<flush;
    for(int i=0;i<nClustersToKeep;i++)
    {
        for(int j=0; j<clusters.at(i).indices.size();j++)
            biggestNClusters.indices.push_back(clusters.at(i).indices.at(j));
    }
    copyPointCloud(*inputCloud,biggestNClusters.indices,*outputCloud);
    cout << "OK! Points remaining: "<<outputCloud->size()<<endl;
}

void keepBiggestCluster(PointCloud<PointXYZI>::Ptr inputCloud,PointCloud<PointXYZI>::Ptr outputCloud,double clusterThreshold,int min,int max)
{
    vector<PointIndices> clusters= findClusters(inputCloud,clusterThreshold,min,max);
    PointIndices biggestCluster;
    cout << "Keeping the biggest cluster of "<< clusters.size() <<" found..."<<flush;
    copyPointCloud(*inputCloud,clusters.at(0).indices,*outputCloud);
    cout << "OK! Points remaining: "<<outputCloud->size()<<endl;
}

float computeLeafSize (PointCloud<PointXYZ>::Ptr cloud,int nVoxelsRequired)
{
    PointXYZ min;
    PointXYZ max;
    //trova gli estremi della cloud
    getMinMax3D<PointXYZ>(*cloud,min,max);
    Eigen::Vector3f size=max.getVector3fMap()-min.getVector3fMap();
    float surface=size[0]*size[1];
    float leafSize= sqrt(2*surface/nVoxelsRequired);
    return leafSize;
}

void onlyNormals(PointCloud<PointXYZ>::Ptr cloud,PointCloud<Normal>::Ptr normals, double radius)
{
    //cout << "Normals estimation... " << flush;
    NormalEstimationOMP<PointXYZ, Normal> normal_estimation;
    normal_estimation.setSearchMethod (search::Search<PointXYZ>::Ptr (new search::KdTree<PointXYZ>));
    normal_estimation.setRadiusSearch (radius);
    normal_estimation.setInputCloud (cloud);//input, ho messo kpts per accelerare
    normal_estimation.compute (*normals);
    //cout << "OK! Normals found: " << normals->points.size() << endl;
}

void onlyNormals(PointCloud<PointXYZI>::Ptr cloud,PointCloud<Normal>::Ptr normals, double radius)
{
    cout << "Normals estimation... " << flush;
    NormalEstimationOMP<PointXYZI, Normal> normal_estimation;
    normal_estimation.setSearchMethod (search::Search<PointXYZI>::Ptr (new search::KdTree<PointXYZI>));
    normal_estimation.setRadiusSearch (radius);
    normal_estimation.setInputCloud (cloud);//input, ho messo kpts per accelerare
    normal_estimation.compute (*normals);
    cout << "OK! Normals found: " << normals->points.size() << endl;
}


void segmentDifferences(PointCloud<PointXYZ>::Ptr source,PointCloud<PointXYZ>::Ptr target,PointCloud<PointXYZ>::Ptr differences,double distanceThreshold)
{
    SegmentDifferences<PointXYZ> segm;
    segm.setInputCloud(source);
    segm.setTargetCloud(target);
    segm.setSearchMethod(search::Search<PointXYZ>::Ptr (new search::KdTree<PointXYZ>));
    segm.setDistanceThreshold(distanceThreshold);
    segm.segment(*differences);
}
